package math2;

import inf.math.LinAlg;

import java.util.ArrayList;
import java.util.Collections;

public class MathUtilities {

	public static final double tol1 = 1e-1;
	public static final double tol3 = 1e-3;
	public static final double tol5 = 1e-5;
	public static final double tol6 = 1e-6;
	public static final double tol8 = 1e-8;
	public static final double tol10 = 1e-10;
	public static final double tol12 = 1e-12;
	public static final double tol14 = 1e-14;

	/**
	 * create an array simulating Matlab
	 * 
	 * @param param
	 *            start1,step1,end1,step2,end2,...
	 * @return lambda array
	 */
	public static double[] createLambdaArray(double... param) {
		ArrayList<Double> lambda = new ArrayList<Double>();
		double current = param[0];
		lambda.add(current);
		int cnt = 1;
		while (true) {
			double step = param[cnt++];
			double end = param[cnt++];
			while (current < end - (step + tol6)) {
				current += step;
				lambda.add(current);
			}
			current = end;
			lambda.add(current);
			if (cnt >= param.length)
				break;
		}
		/*
		 * convert to double array
		 */
		double[] l = new double[lambda.size()];
		for (int i = 0; i < l.length; i++) {
			l[i] = lambda.get(i);
		}
		return l;
	}

	/**
	 * create an array with repetition
	 * 
	 * @param values
	 *            array of values [v1, v2, v3, ...]
	 * @param repetition
	 *            array of repetition [n1, n2, n3, ...] (ni can be 0)
	 * @return [v1, ...(n1 times)..., v1, v2, ...(n2 times)..., v2, ...]
	 */
	public static double[] createLambdaArray(double[] values, int[] repetition) {
		assert values.length == repetition.length : "the values array and repetition array must be of the same length";
		ArrayList<Double> lambda = new ArrayList<Double>();
		for (int i = 0; i < values.length; i++) {
			for (int j = 0; j < repetition[i]; j++) {
				lambda.add(values[i]);
			}
		}
		/*
		 * convert to double array
		 */
		double[] l = new double[lambda.size()];
		for (int i = 0; i < l.length; i++) {
			l[i] = lambda.get(i);
		}
		return l;
	}

	/**
	 * compute sqrt(a^2 + b^2) without under/overflow.
	 * 
	 * @param a
	 * @param b
	 * @return
	 */
	public static double hypot(double a, double b) {
		double r;
		if (Math.abs(a) > Math.abs(b)) {
			r = b / a;
			r = Math.abs(a) * Math.sqrt(1 + r * r);
		} else if (b != 0) {
			r = a / b;
			r = Math.abs(b) * Math.sqrt(1 + r * r);
		} else {
			r = 0.0;
		}
		return r;
	}

	/**
	 * compute triangle area using Heron formula
	 * 
	 * @param X
	 * @param Y
	 * @param Z
	 * @return
	 */
	public static double heron(double[] X, double[] Y, double[] Z) {
		int dim = X.length;
		double a = LinAlg.diffNorm2(dim, X, Y);
		double b = LinAlg.diffNorm2(dim, Y, Z);
		double c = LinAlg.diffNorm2(dim, X, Z);
		double s = (a + b + c) / 2;
		return Math.sqrt(s * (s - a) * (s - b) * (s - c));
	}

	/**
	 * compute cross product of 2 vectors
	 * 
	 * @param X
	 * @param Y
	 * @return 3d vector of cross product
	 */
	public static double[] cross(double[] X, double[] Y) {
		int dim = X.length;
		assert Y.length == dim : "incompatible dimension";
		double[] z = new double[dim];
		if (dim == 3) {
			z[0] = X[1] * Y[2] - X[2] * Y[1];
			z[1] = X[2] * Y[0] - X[0] * Y[2];
			z[2] = X[0] * Y[1] - X[1] * Y[0];
		} else {
			z[0] = 0;
			z[1] = 0;
			z[2] = X[0] * Y[1] - X[1] * Y[0];
		}
		return z;
	}

	/**
	 * compute product of 2 vectors in 3d
	 * 
	 * @param x
	 * @param y
	 * @return
	 */
	public static double[] cross(double x1, double x2, double x3, double y1, double y2, double y3) {
		double[] z = new double[3];
		z[0] = x2 * y3 - x3 * y2;
		z[1] = x3 * y1 - x1 * y3;
		z[2] = x1 * y2 - x2 * y1;
		return z;
	}

	/**
	 * compute intersection of two straight line defined by a1*x+b1*y+c1=0 and
	 * a2*x+b2*y+c2=0
	 * 
	 * @param a1
	 * @param b1
	 * @param c1
	 * @param a2
	 * @param b2
	 * @param c2
	 * @return
	 */
	public static double[] computeIntersection(double a1, double b1, double c1, double a2,
			double b2, double c2) {
		double det = a1 * b2 - b1 * a2;
		double x = (b1 * c2 - b2 * c1) / det;
		double y = (a2 * c1 - a1 * c2) / det;
		return new double[] { x, y };
	}

	/**
	 * compute intersection of surface ax+by+cz+d=0 with line (x0,y0,z0)/(u,v,w)
	 * 
	 * @param a
	 * @param b
	 * @param c
	 * @param d
	 * @param x0
	 * @param y0
	 * @param z0
	 * @param u
	 * @param v
	 * @param w
	 * @return
	 */
	public static double[] computeIntersection(double a, double b, double c, double d, double x0,
			double y0, double z0, double u, double v, double w) {
		double tmp = a * u + b * v + c * w;
		if (Math.abs(tmp) < tol6) {
			throw new Error("line and surface are parallel");
		}
		double t = -(a * x0 + b * y0 + c * z0 + d) / tmp;
		return new double[] { x0 + u * t, y0 + v * t, z0 + w * t };
	}

	/**
	 * compute intersection of surface ax+by+c = 0 with line (x0, y0)/(u, v)
	 * 
	 * @param a
	 * @param b
	 * @param c
	 * @param x0
	 * @param y0
	 * @param u
	 * @param v
	 * @return
	 */
	public static double[] computeIntersection(double a, double b, double c, double x0, double y0,
			double u, double v) {
		double tmp = a * u + b * v;
		if (Math.abs(tmp) < tol6) {
			throw new Error("line and line are parallel");
		}
		double t = -(a * x0 + b * y0 + c) / tmp;
		return new double[] { x0 + u * t, y0 + v * t };
	}

	/**
	 * get the indices of sorted array in given order. Note that the array will
	 * not change
	 * 
	 * @param array
	 * @param order
	 *            if order == false: sort ascending; if order == true: sort
	 *            descending
	 * @return
	 */
	public static int[] sort(double[] array, boolean order) {

		class Pair implements Comparable<Pair> {

			private int i;
			private double v;

			public Pair(int i, double v) {
				this.i = i;
				this.v = v;
			}

			public int getId() {
				return i;
			}

			@Override
			public int compareTo(Pair o) {
				return Double.compare(v, o.v);
			}

		}

		ArrayList<Pair> list = new ArrayList<Pair>();
		for (int i = 0; i < array.length; i++) {
			Pair p = new Pair(i, array[i]);
			list.add(p);
		}

		Collections.sort(list);

		int[] indices = new int[array.length];
		if (!order) {
			for (int i = 0; i < indices.length; i++) {
				indices[i] = list.get(i).getId();
			}
		} else {
			for (int i = 0; i < indices.length; i++) {
				indices[i] = list.get(indices.length - 1 - i).getId();
			}
		}

		return indices;
	}

	/**
	 * find the element in the array based on indices
	 * 
	 * @param array
	 * @param ids
	 * @return
	 */
	public static double[] find(double[] array, int... ids) {
		double[] v = new double[ids.length];
		for (int i = 0; i < v.length; i++) {
			assert ids[i] < array.length : "index exceed length of the array";
			v[i] = array[ids[i]];
		}
		return v;
	}
}
